using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;

//using arm = Microsoft.Robotics.Services.ArticulatedArm.Proxy;
using lynx = ProMRDS.Robotics.Lynxmotion.Lynx6Arm.Proxy;
using Microsoft.Robotics.PhysicalModel.Proxy;
using System.IO;
using physicalmodel = Microsoft.Robotics.PhysicalModel;


namespace ProMRDS.Robotics.MotionRecorder
{
    public partial class MotionRecorderUI : Form
    {
        // Port to use to communicate with the main service
        FromWinformEvents _fromWinformPort;

        // Short term memory for Save/Restore of Pose
        float _memX;
        float _memY;
        float _memZ;
        float _memGripAngle;
        float _memGripRotation;
        float _memGrip;
        float _memTime;
        float[] _memAngles = null;

        string[] _jointNames = null;
        float[] _previousAngles = null;

        // The maximum angular speed is used to calculate the delays
        // This should be the same value as in the Lynx 6 arm service
        // Ideally, we could get the value from the service but there
        // are too many steps involved!
        float maxAngularSpeed = 20.0f;

        // Do NOT use the default constructor!
        //public MotionRecorderUI()
        //{
        //    InitializeComponent();
        //}

        public MotionRecorderUI(FromWinformEvents EventsPort)
        {
            // Save the port - we need it to send requests to the service
            _fromWinformPort = EventsPort;
            // Must initialize the Form
            InitializeComponent();
            // Send a message to say that the init is now complete
            // This also passes back a handle to the Form
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.Loaded, null, this));
        }

        // This is not really required ...
        private void MotionRecorderUI_Load(object sender, EventArgs e)
        {
        }

        // Exit button handler
        // NOTE: This is misleading because you can't shut down a DssHost in
        // version 1.5 of MRDS so the service is dropped, but the DssHost just
        // keeps on running ... Maybe in a future version?
        private void _exitButton_Click(object sender, EventArgs e)
        {
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.Exit, null));
            this.Close();
        }

        #region Service Functions

        // Functions executed by the main service

        public void SetErrorText(string error)
        {
            _errorLabel.Text = error;
        }

        // Insert the full set of pose values into the textboxes
        public void SetPoseText(float x, float y, float z, float p, float w, float grip, float time)
        {
            _xText.Text = x.ToString("f2");
            _yText.Text = y.ToString("f2");
            _zText.Text = z.ToString("f2");
            _gripAngleText.Text = p.ToString();
            _gripRotationText.Text = w.ToString();
            _gripText.Text = grip.ToString();
            _timeText.Text = time.ToString();
        }

        // Set the values of the joint sliders and textboxes
        // NOTE: This can be done at any time
        public void SetSliders(float baseAngle, float shoulderAngle, float elbowAngle,
                float wristAngle, float wristRotateAngle, float gripperAngle)
        {
            try
            {
                _baseTrackBar.Value = (int)Math.Round(baseAngle);
                _baseText.Text = _baseTrackBar.Value.ToString();
                _shoulderTrackBar.Value = (int)Math.Round(shoulderAngle);
                _shoulderText.Text = _shoulderTrackBar.Value.ToString();
                _elbowTrackBar.Value = (int)Math.Round(elbowAngle);
                _elbowText.Text = _elbowTrackBar.Value.ToString();
                _wristTrackBar.Value = (int)Math.Round(wristAngle);
                _wristText.Text = _wristTrackBar.Value.ToString();
                _wristRotateTrackBar.Value = (int)Math.Round(wristRotateAngle);
                _wristRotateText.Text = _wristRotateTrackBar.Value.ToString();
                _gripperTrackBar.Value = (int)Math.Round(gripperAngle);
                _gripperText.Text = _gripperTrackBar.Value.ToString();
            }
            catch
            {
                _errorLabel.Text = "Error setting trackbars";
            }
        }

        // Display the arm end effector position
        public void UpdateArmPose(Pose p)
        {
            float temp;
            temp = p.Position.X * 100;
            _poseXText.Text = temp.ToString("f2");
            temp = p.Position.Y * 100;
            _poseYText.Text = temp.ToString("f2");
            temp = p.Position.Z * 100;
            _poseZText.Text = temp.ToString("f2");

            float gripAngle;
            physicalmodel.AxisAngle a;
            physicalmodel.Quaternion q = new physicalmodel.Quaternion(
                p.Orientation.X,
                p.Orientation.Y,
                p.Orientation.Z,
                p.Orientation.W);
            a = physicalmodel.Quaternion.ToAxisAngle(q);
            gripAngle = Conversions.RadiansToDegrees(a.Angle * Math.Sign(a.Axis.X));
            _posePText.Text = gripAngle.ToString("f2");
        }

        // Set up the list of joints
        // IMPORTANT: This must be called before anything else can be
        // done! It creates a couple of key arrays. There will be
        // access violations if anything else happens first.
        public void ReplaceArticulatedArmJointList(string[] joints)
        {
            // Create a new array if necessary
            if (_jointNames == null)
                _jointNames = new string[joints.Length];

            // Copy the names
            if (joints.Length > 0)
            {
                for (int i=0; i<joints.Length; i++)
                    _jointNames[i] = joints[i];
            }
            else
            {
                _jointNames = null;
            }

            // Create an array to keep track of where the arm
            // currently is
            _previousAngles = new float[_jointNames.Length];

            // Initialize the previous angles
            // NOTE: This cannot be done until the first Get of
            // the arm state has completed because it updates the
            // trackbars to match the actual arm angles.
            _previousAngles = new float[_jointNames.Length];
            _previousAngles[(int)lynx.JointNumbers.Base] = (float)_baseTrackBar.Value;
            _previousAngles[(int)lynx.JointNumbers.Shoulder] = (float)_shoulderTrackBar.Value;
            _previousAngles[(int)lynx.JointNumbers.Elbow] = (float)_elbowTrackBar.Value;
            _previousAngles[(int)lynx.JointNumbers.Wrist] = (float)_wristTrackBar.Value;
            _previousAngles[(int)lynx.JointNumbers.WristRotate] = (float)_wristRotateTrackBar.Value;
            _previousAngles[(int)lynx.JointNumbers.Gripper] = (float)_gripperTrackBar.Value;

        }

        #endregion

        #region Checking

        // Change any blank fields to a zero so that there are no errors
        // when parsing the fields as numbers
        private void CheckForBlanks()
        {
            if (_xText.Text == "")
                _xText.Text = "0";
            if (_yText.Text == "")
                _yText.Text = "0";
            if (_zText.Text == "")
                _zText.Text = "0";
            if (_gripAngleText.Text == "")
                _gripAngleText.Text = "0";
            if (_gripRotationText.Text == "")
                _gripRotationText.Text = "0";
            if (_gripText.Text == "")
                _gripText.Text = "0";
            if (_timeText.Text == "")
                _timeText.Text = "0";

            if (_baseText.Text == "")
                _baseText.Text = "0";
            if (_shoulderText.Text == "")
                _shoulderText.Text = "0";
            if (_elbowText.Text == "")
                _elbowText.Text = "0";
            if (_wristText.Text == "")
                _wristText.Text = "0";
            if (_wristRotateText.Text == "")
                _wristRotateText.Text = "0";
            if (_gripperText.Text == "")
                _gripperText.Text = "0";
        }

        // Check all of the joint angles
        // NOTE: The min and max are set on the trackbars in the Designer
        // and not in the code! These values are somewhat arbitrary, but
        // are intended to make the arm operate safely.
        bool ValidateJointAngles(
                    string baseText,
                    string shoulderText,
                    string elbowText,
                    string wristText,
                    string wristRotateText,
                    string gripperText,
                    ref float[] Angles)
        {
            float val;

            _errorLabel.Text = string.Empty;

            try
            {
                val = Single.Parse(baseText);
                if (val < _baseTrackBar.Minimum || val > _baseTrackBar.Maximum)
                    throw (new Exception("Base Out of Range"));
                Angles[(int)lynx.JointNumbers.Base] = val;

                val = Single.Parse(shoulderText);
                if (val < _shoulderTrackBar.Minimum || val > _shoulderTrackBar.Maximum)
                    throw (new Exception("Shoulder Out of Range"));
                Angles[(int)lynx.JointNumbers.Shoulder] = val;

                val = Single.Parse(elbowText);
                if (val < _elbowTrackBar.Minimum || val > _elbowTrackBar.Maximum)
                    throw (new Exception("Elbow Out of Range"));
                Angles[(int)lynx.JointNumbers.Elbow] = val;

                val = Single.Parse(wristText);
                if (val < _wristTrackBar.Minimum || val > _wristTrackBar.Maximum)
                    throw (new Exception("Wrist Out of Range"));
                Angles[(int)lynx.JointNumbers.Wrist] = val;

                val = Single.Parse(wristRotateText);
                if (val < _wristRotateTrackBar.Minimum || val > _wristRotateTrackBar.Maximum)
                    throw (new Exception("Wrist Rotate Out of Range"));
                Angles[(int)lynx.JointNumbers.WristRotate] = val;

                val = Single.Parse(gripperText);
                if (val < _gripperTrackBar.Minimum || val > _gripperTrackBar.Maximum)
                    throw (new Exception("Gripper Out of Range"));
                Angles[(int)lynx.JointNumbers.Gripper] = val;

                return true;
            }
            catch (Exception ex)
            {
                // Display the error
                _errorLabel.Text = ex.Message;
                return false;
            }
        }


        // Limits on the range of Pose values are as explained below.
        // The coordinate system is as follows with the center
        // of the base as the origin.
        // Looking at the arm side on facing the side where the switch and
        // serial connector are then:
        // X axis increases to the right, which means X is ALWAYS negative
        // Y axis increases upwards
        // Z axis increases towards you, so negative is on the other side
        // of the arm away from you.
        // All values are in centimeters.
        float minX = -35;
        float maxX = 0;
        float minY = 1;
        float maxY = 40;
        float minZ = -35;
        float maxZ = 35;
        // NOTE: This is NOT the same as the wrist joint angle
        // It is the angle to the horizontal, which is much more
        // useful for positioning the gripper, e.g. zero means
        // that the gripper should be horizontal.
        float minWristAngle = -80;
        float maxWristAngle = 80;
        // These should be the same as on the corresponding trackbars
        float minWristRotate = -70;
        float maxWristRotate = +70;
        float minGripper = -35;
        float maxGripper = 35;


        // Check all of the Pose details
        // These values are intended to make the arm operate safely.
        bool ValidatePose(
                    string xText,
                    string yText,
                    string zText,
                    string wristAngleText,
                    string wristRotateText,
                    string gripperText,
                    ref float[] Values)
        {
            float val;

            _errorLabel.Text = string.Empty;

            try
            {
                int i = 0;

                val = Single.Parse(xText);
                if (val < minX || val > maxX)
                    throw (new Exception("X Out of Range"));
                Values[i++] = val;

                val = Single.Parse(yText);
                if (val < minY || val > maxY)
                    throw (new Exception("Y Out of Range"));
                Values[i++] = val;

                val = Single.Parse(zText);
                if (val < minZ || val > maxZ)
                    throw (new Exception("Z Out of Range"));
                Values[i++] = val;

                val = Single.Parse(wristAngleText);
                if (val < minWristAngle || val > maxWristAngle)
                    throw (new Exception("Wrist Angle Out of Range"));
                Values[i++] = val;

                val = Single.Parse(wristRotateText);
                if (val < _wristRotateTrackBar.Minimum || val > _wristRotateTrackBar.Maximum)
                    throw (new Exception("Wrist Rotate Out of Range"));
                Values[i++] = val;

                val = Single.Parse(gripperText);
                if (val < _gripperTrackBar.Minimum || val > _gripperTrackBar.Maximum)
                    throw (new Exception("Gripper Out of Range"));
                Values[i++] = val;

                return true;
            }
            catch (Exception ex)
            {
                // Display the error
                _errorLabel.Text = ex.Message;
                return false;
            }
        }

        #endregion

        #region Motion Execution

        // Execute a motion based on either the pose or joint angles
        private void _executeButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            // Make sure that the user has not erased any values
            CheckForBlanks();

            if (_poseRadio.Checked)
            {
                // Handle a pose
                try
                {
                    MoveToPositionParameters moveParams = new MoveToPositionParameters();

                    float[] Values = new float[_jointNames.Length];

                    // Sanity check the values first
                    if (ValidatePose(
                                _xText.Text,
                                _yText.Text,
                                _zText.Text,
                                _gripAngleText.Text,
                                _gripRotationText.Text,
                                _gripText.Text,
                                ref Values))
                    {
                        // Put the parameters into the request
                        // Convert the coords from cm to meters
                        moveParams.X = Values[0] / 100;
                        moveParams.Y = Values[1] / 100;
                        moveParams.Z = Values[2] / 100;
                        // Leave the angles alone
                        moveParams.GripAngle = Values[3];
                        moveParams.GripRotation = Values[4];
                        moveParams.Grip = Values[5];
                        moveParams.Time = Single.Parse(_timeText.Text);

                        // Try to move the arm
                        // NOTE: There is no guarantee that the specified pose
                        // is reachable!
                        _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveToPosition, null, moveParams));
                    }
                }
                catch
                {
                    _errorLabel.Text = "Invalid Value";
                }
            }
            else
            {
                // Handle joint angles
                float[] Angles = new float[_jointNames.Length];
                if (ValidateJointAngles(
                            _baseText.Text,
                            _shoulderText.Text,
                            _elbowText.Text,
                            _wristText.Text,
                            _wristRotateText.Text,
                            _gripperText.Text,
                            ref Angles))
                {
                    // Update the trackbars to match the textboxes now that they have
                    // all been validated
                    // NOTE: When moving the trackbar, the textbox is kept in synch so
                    // putting the values back does no harm. However, if the user has
                    // edited a textbox then we need to synch.
                    _baseTrackBar.Value = (int)Math.Round(Angles[(int)lynx.JointNumbers.Base]);
                    _shoulderTrackBar.Value = (int)Math.Round(Angles[(int)lynx.JointNumbers.Shoulder]);
                    _elbowTrackBar.Value = (int)Math.Round(Angles[(int)lynx.JointNumbers.Elbow]);
                    _wristTrackBar.Value = (int)Math.Round(Angles[(int)lynx.JointNumbers.Wrist]);
                    _wristRotateTrackBar.Value = (int)Math.Round(Angles[(int)lynx.JointNumbers.WristRotate]);
                    _gripperTrackBar.Value = (int)Math.Round(Angles[(int)lynx.JointNumbers.Gripper]);

                    // Now move the arm
                    MoveMultipleJointParameters moveParams = new MoveMultipleJointParameters(_jointNames, Angles);
                    _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveMultipleJoints, null, moveParams));
                }
            }

        }

        // Move the arm to the "park" position (whatever that is!)
        private void _parkButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.Park, null, null));
        }

        // Reset the arm so all angles are zero
        private void _resetButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.Reset, null));
            _baseText.Text = "0";
            _shoulderText.Text = "0";
            _elbowText.Text = "0";
            _wristText.Text = "0";
            _wristRotateText.Text = "0";
            _gripperText.Text = "0";
        }

        // Get the current pose of the arm from the arm service
        // NOTE: This requests the pose, and the some time later the values
        // in the Form will be updated. It is NOT synchronous.
        private void _getPoseButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.GetPose, null, null));
        }

        #endregion

        #region Command List Editng

        // Copy the pose information to the pose textboxes
        private void _copyPoseButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;
            _xText.Text = _poseXText.Text;
            _yText.Text = _poseYText.Text;
            _zText.Text = _poseZText.Text;
            _gripAngleText.Text = _posePText.Text;
        }

        // Copy the joint angles from the currently selected command
        // in the command list
        private void _copyJointsButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            // Make sure that a command has been selected
            if (_commandsListBox.SelectedIndex < 0)
            {
                MessageBox.Show(this, "Please select a command in the list", "Warning", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
                return;
            }
            else
            {
                // Process the command
                string cmd;
                string[] fields;
                cmd = _commandsListBox.Items[_commandsListBox.SelectedIndex].ToString();
                fields = cmd.Split(',');
                if (fields.Length < 8)
                {
                    MessageBox.Show(this, "Not enough command parameters", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                    return;
                }

                // Only Move commands are acceptable
                if (fields[0].ToLower() == "move")
                {
                    float[] Angles = new float[_jointNames.Length];

                    // Check the angles
                    // Why? Because the data might have been loaded from a file
                    // and we have no idea what the user might have done to it!
                    // NOTE: The time is not currently copied so it is not checked
                    if (!ValidateJointAngles(
                                fields[1],
                                fields[2],
                                fields[3],
                                fields[4],
                                fields[5],
                                fields[6],
                                ref Angles))
                    {
                        MessageBox.Show(this, "Invalid Angle", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                        return;
                    }

                    // Now set the sliders to reflect the changes
                    // This sets the textboxes too
                    SetSliders(
                            Angles[(int)lynx.JointNumbers.Base],
                            Angles[(int)lynx.JointNumbers.Shoulder],
                            Angles[(int)lynx.JointNumbers.Elbow],
                            Angles[(int)lynx.JointNumbers.Wrist],
                            Angles[(int)lynx.JointNumbers.WristRotate],
                            Angles[(int)lynx.JointNumbers.Gripper]);

                    // Set the time as well
                    _timeText.Text = fields[7];
                }
                else if (fields[0].ToLower() == "pose")
                {
                    float[] Values = new float[_jointNames.Length];

                    // Check the pose values
                    // Why? Because the data might have been loaded from a file
                    // and we have no idea what the user might have done to it!
                    // NOTE: The time is not currently copied so it is not checked
                    if (!ValidatePose(
                                fields[1],
                                fields[2],
                                fields[3],
                                fields[4],
                                fields[5],
                                fields[6],
                                ref Values))
                    {
                        MessageBox.Show(this, "Invalid Pose", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                        return;
                    }

                    // Now set the pose parameters to reflect the changes
                    _xText.Text = Values[0].ToString("f2");
                    _yText.Text = Values[1].ToString("f2");
                    _zText.Text = Values[2].ToString("f2");
                    _gripAngleText.Text = Values[3].ToString("f2");
                    _gripRotationText.Text = Values[4].ToString("f2");
                    _gripText.Text = Values[5].ToString("f2");

                    // Set the time as well
                    _timeText.Text = fields[7];
                }
                else
                {
                    MessageBox.Show(this, "Please select a Move or Pose command in the list", "Warning", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
                    return;
                }
            }

        }

        // Double-click is a shortcut for Copy
        private void _commandsListBox_DoubleClick(object sender, EventArgs e)
        {
            _copyJointsButton_Click(sender, e);
        }

        // Save the current pose information temporarily
        private void _saveButton_Click(object sender, EventArgs e)
        {
            Single x, y, z, gripAngle, gripRotation, grip, time;

            _errorLabel.Text = string.Empty;

            try
            {
                _errorLabel.Text = string.Empty;
                float[] Angles = new float[_jointNames.Length];

                if (!ValidateJointAngles(
                            _baseText.Text,
                            _elbowText.Text,
                            _shoulderText.Text,
                            _wristText.Text,
                            _wristRotateText.Text,
                            _gripperText.Text,
                            ref Angles))
                    return;

                _memAngles = Angles;

                x = Single.Parse(_xText.Text);
                y = Single.Parse(_yText.Text);
                z = Single.Parse(_zText.Text);
                gripAngle = Single.Parse(_gripAngleText.Text);
                gripRotation = Single.Parse(_gripRotationText.Text);
                grip = Single.Parse(_gripText.Text);
                time = Single.Parse(_timeText.Text);
                // Succeeded in parsing value so set them now
                _memX = x;
                _memY = y;
                _memZ = z;
                _memGripAngle = gripAngle;
                _memGripRotation = gripRotation;
                _memGrip = grip;
                _memTime = time;
            }
            catch
            {
                _errorLabel.Text = "Invalid Value";
            }

        }

        // Restore the saved pose information
        private void _restoreButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            SetPoseText(_memX, _memY, _memZ, _memGripAngle, _memGripRotation, _memGrip, _memTime);
            SetSliders(
                _memAngles[(int)lynx.JointNumbers.Base],
                _memAngles[(int)lynx.JointNumbers.Shoulder],
                _memAngles[(int)lynx.JointNumbers.Elbow],
                _memAngles[(int)lynx.JointNumbers.Wrist],
                _memAngles[(int)lynx.JointNumbers.WristRotate],
                _memAngles[(int)lynx.JointNumbers.Gripper]
                );

        }

        string MakeCommandString()
        {
            string line;

            // Construct a comma separated values list from the info on
            // the Form
            if (_poseRadio.Checked)
            {
                line = "Pose," + _xText.Text + "," + _yText.Text + "," +
                        _zText.Text + "," + _gripAngleText.Text + "," +
                        _gripRotationText.Text + "," + _gripText.Text;
            }
            else
            {
                line = "Move," + _baseText.Text + "," + _shoulderText.Text + "," +
                        _elbowText.Text + "," + _wristText.Text + "," +
                        _wristRotateText.Text + "," + _gripperText.Text;
            }
            // Append the time parameter
            line += "," + _timeText.Text;

            return line;
        }

        // Record the current command in the command history list
        private void _appendButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            _playing = false;

            // Build the command
            string line = MakeCommandString();

            // Add it to the command history list
            _commandsListBox.Items.Add(line);
        }

        private void _insertButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            _playing = false;
            if (_commandsListBox.SelectedIndex >= 0)
            {
                string line = MakeCommandString();
                _commandsListBox.Items.Insert(_commandsListBox.SelectedIndex, line);
            }
            else
                MessageBox.Show(this, "Please select an item in the list. Insert will be above this item.", "Warning", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
        }

        private void _replaceButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            _playing = false;
            if (_commandsListBox.SelectedIndex >= 0)
            {
                string line = MakeCommandString();
                int index = _commandsListBox.SelectedIndex;
                _commandsListBox.Items.RemoveAt(index);
                _commandsListBox.Items.Insert(index, line);
            }
            else
                MessageBox.Show(this, "Please select an item to replace from the list", "Warning", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
        }

        private void _execute2Button_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            _playing = false;
            // Make sure that a command has been selected
            if (_commandsListBox.SelectedIndex < 0)
            {
                MessageBox.Show(this, "Please select an item to execute from the list", "Warning", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
                return;
            }
            else
            {
                // Process the command
                string cmd;
                string[] fields;
                float[] angles;
                int delay;
                cmd = _commandsListBox.Items[_commandsListBox.SelectedIndex].ToString();
                fields = cmd.Split(',');
                if (fields.Length < 8)
                {
                    MessageBox.Show(this, "Not enough command parameters", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
                    return;
                }
                ExecuteCommand(cmd, out angles, out delay);
            }
        }

        // Delete the currently selected command from the history list
        private void _deleteButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            _playing = false;
            if (_commandsListBox.SelectedIndex >= 0)
            {
                _commandsListBox.Items.RemoveAt(_commandsListBox.SelectedIndex);
            }
            else
                MessageBox.Show(this, "Please select an item to delete from the list", "Warning", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
        }

        // Insert a delay into the command history list
        private void _delayButton_Click(object sender, EventArgs e)
        {
            string line;

            _errorLabel.Text = string.Empty;

            _playing = false;
            // NOTE: The Delay uses the current joint angles, but when it
            // is replayed these are ignored. Perhaps it should just insert
            // a bunch of zeros.
            line = "Delay," + _baseText.Text + "," + _shoulderText.Text + "," +
                    _elbowText.Text + "," + _wristText.Text + "," +
                    _wristRotateText.Text + "," + _gripperText.Text + "," +
                    _timeText.Text;
            _commandsListBox.Items.Add(line);
        }

        #endregion

        #region Playback Functions

        // Global flag for playback
        bool _playing = false;
        int _currentCommandIndex = 0;

        // Play all the commands in the history list from the beginning
        private void _playButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            _playing = false;
            _playbackTimer.Enabled = false;

            if (_commandsListBox.Items.Count <= 0)
            {
                MessageBox.Show(this, "Please record some commands or load a file", "Warning", MessageBoxButtons.OK, MessageBoxIcon.Exclamation);
                return;
            }

            _playing = true;

            // Assume we are starting at the beginning
            _currentCommandIndex = 0;
            // If the selected item is NOT at the beginning
            if (_commandsListBox.SelectedIndex > 0)
            {
                DialogResult result;
                // See if the user wants to start at the beginning
                result = MessageBox.Show(this, "Start from the beginning?", "Start Playback", MessageBoxButtons.YesNoCancel, MessageBoxIcon.Question);
                if (result == DialogResult.Cancel)
                    return;
                else if (result == DialogResult.No)
                    // Start with the selected item
                    _currentCommandIndex = _commandsListBox.SelectedIndex;
            }

            // Kick off the timer
            _playbackTimer.Interval = 100;
            _playbackTimer.Enabled = true;
        }

        // Stop playing
        // If playing is in progress, the current command will still complete
        private void _stopButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            // Set the flag so that the playback stops on the next timer tick
            _playing = false;
            _playbackTimer.Enabled = false;
            MessageBox.Show(this, "Playback stopped", "Info", MessageBoxButtons.OK, MessageBoxIcon.Information);
            return;
        }

        // Timer for executing the commands during playback
        private void _playbackTimer_Tick(object sender, EventArgs e)
        {
            float[] angles;
            int delay;
            int timer;
            float maxDiff, diff;

            _playbackTimer.Enabled = false;

            // Stop if playback has been turned off
            if (!_playing)
            {
                MessageBox.Show(this, "Playback stopped", "Info", MessageBoxButtons.OK, MessageBoxIcon.Information);
                return;
            }

            if (_currentCommandIndex >= _commandsListBox.Items.Count)
            {
                MessageBox.Show(this, "Playback complete", "Info", MessageBoxButtons.OK, MessageBoxIcon.Information);
                return;
            }

            _commandsListBox.SelectedIndex = _currentCommandIndex;
            if (!ExecuteCommand(_commandsListBox.Items[_currentCommandIndex].ToString(), out angles, out delay))
            {
                MessageBox.Show(this, "Playback terminated", "Info", MessageBoxButtons.OK, MessageBoxIcon.Information);
                return;
            }

            // Calculate the delay for this motion
            maxDiff = -1;
            for (int i = 0; i < _previousAngles.Length; i++)
            {
                diff = Math.Abs(angles[i] - _previousAngles[i]);
                if (diff > maxDiff)
                    maxDiff = diff;
                _previousAngles[i] = angles[i];
            }
            timer = (int)((maxDiff / maxAngularSpeed) * 1000.0f);
            if (timer < 10)
                timer = 20;
            if (delay > timer)
                timer = delay;

            // Increment the index
            _currentCommandIndex++;

            // Restart the timer
            _playbackTimer.Interval = timer;
            _playbackTimer.Enabled = true;

        }

        // Execute a command in comma separated values format
        // This can be either a Pose, Move or Delay command
        private bool ExecuteCommand(string cmd, out float[] angles, out int delay)
        {
            string[] fields;

            angles = new float[_jointNames.Length];
            delay = 0;

            // Split up the parameters
            fields = cmd.Split(',');

            if (fields.Length < 8)
            {
                if (MessageBox.Show(this, "Not enough command parameters\r\nDo you wish to continue?", "Command Error", MessageBoxButtons.YesNo, MessageBoxIcon.Question) == DialogResult.Yes)
                    return true;
                else
                    return false;
            }

            try
            {
                delay = int.Parse(fields[7]);
                if (delay < 0)
                    delay = 0;
            }
            catch
            {
                if (MessageBox.Show(this, "Invalid time delay\r\nDo you wish to continue?", "Command Error", MessageBoxButtons.YesNo, MessageBoxIcon.Question) != DialogResult.Yes)
                    return false;
                delay = 1000;
            }

            // Only Move commands are acceptable
            if (fields[0].ToLower() == "move")
            {

                // Check the angles
                // Why? Because the data might have been loaded from a file
                // and we have no idea what the user might have done to it!
                // NOTE: The time is not currently copied so it is not checked
                if (!ValidateJointAngles(
                            fields[1],
                            fields[2],
                            fields[3],
                            fields[4],
                            fields[5],
                            fields[6],
                            ref angles))
                {
                    if (MessageBox.Show(this, "Invalid Joint Angle\r\nDo you wish to continue?", "Command Error", MessageBoxButtons.YesNo, MessageBoxIcon.Question) == DialogResult.Yes)
                        return true;
                    else
                        return false;
                }
                    
                // Execute the move
                MoveMultipleJointParameters moveParams = new MoveMultipleJointParameters(_jointNames, angles);
                _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveMultipleJoints, null, moveParams));
            }
            else if (fields[0].ToLower() == "pose")
            {
                float[] Values = new float[_jointNames.Length];

                // Sanity check the values first
                if (!ValidatePose(
                            fields[1],
                            fields[2],
                            fields[3],
                            fields[4],
                            fields[5],
                            fields[6],
                            ref Values))
                {
                    if (MessageBox.Show(this, "Invalid Pose Info\r\nDo you wish to continue?", "Command Error", MessageBoxButtons.YesNo, MessageBoxIcon.Question) == DialogResult.Yes)
                        return true;
                    else
                        return false;
                }
                else
                {
                    MoveToPositionParameters moveParams = new MoveToPositionParameters();

                    // Put the parameters into the request
                    // Convert the coords from cm to meters
                    moveParams.X = Values[0] / 100;
                    moveParams.Y = Values[1] / 100;
                    moveParams.Z = Values[2] / 100;
                    // Leave the angles alone
                    moveParams.GripAngle = Values[3];
                    moveParams.GripRotation = Values[4];
                    moveParams.Grip = Values[5];
                    moveParams.Time = Single.Parse(_timeText.Text);

                    // Try to move the arm
                    // NOTE: There is no guarantee that the specified pose
                    // is reachable!
                    _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveToPosition, null, moveParams));
                }
            }
            else if (fields[0].ToLower() == "delay")
            {
                // Do nothing - the time delay will happen anyway
            }
            else
            {
                // The command is not valid!
                if (MessageBox.Show(this, "Invalid Command: " + fields[0] + "\r\nDo you wish to continue?", "Command Error", MessageBoxButtons.YesNo, MessageBoxIcon.Question) == DialogResult.Yes)
                    return true;
                else
                    return false;
            }
            return true;
        }

        #endregion

        #region File IO

        // Save the current command sequence to a file
        private void _saveFileButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            if (_saveFileDialog.ShowDialog() != DialogResult.Cancel)
            {
                try
                {
                    int counter = 0;
                    string line;

                    StreamWriter file = new StreamWriter(_saveFileDialog.FileName);
                    {
                        for (int i = 0; i < _commandsListBox.Items.Count; i++)
                        {
                            line = _commandsListBox.Items[i].ToString();
                            file.WriteLine(line);
                            counter++;
                        }
                    }

                    file.Close();
                    MessageBox.Show(this, "Wrote " + counter + " lines to file " + _saveFileDialog.FileName);
                }
                catch (Exception ex)
                {
                    MessageBox.Show(this, "Error writing to file: " + ex.Message);
                }
                
            }

        }

        // Load a saved command sequence file
        // NOTE: The file is NOT checked for validity and you can load
        // any file! However, when you try to play it there will be errors.
        private void _loadFileButton_Click(object sender, EventArgs e)
        {
            _errorLabel.Text = string.Empty;

            if (_openFileDialog.ShowDialog() != DialogResult.Cancel)
            {
                try
                {
                    int counter = 0;
                    string line;

                    StreamReader file = new StreamReader(_openFileDialog.FileName);
                    _commandsListBox.Items.Clear();
                    while ((line = file.ReadLine()) != null)
                    {
                        _commandsListBox.Items.Add(line);
                        counter++;
                    }

                    file.Close();
                    MessageBox.Show(this, "Read " + counter + " lines from file " + _openFileDialog.FileName);
                }
                catch (Exception ex)
                {
                    MessageBox.Show(this, "Error reading from file: " + ex.Message);
                }
                
            }
        }

        #endregion

        #region TrackBar Handlers

        private void _gripperTrackBar_Scroll(object sender, EventArgs e)
        {
            TrackBar tb = (TrackBar)sender;
            float angle = (float)tb.Value;
            _gripperText.Text = tb.Value.ToString();
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveJoint, null,
                new MoveJointParameters(_jointNames[(int)lynx.JointNumbers.Gripper], angle)));
        }

        private void _wristRotateTrackBar_Scroll(object sender, EventArgs e)
        {
            TrackBar tb = (TrackBar)sender;
            float angle = (float)tb.Value;
            _wristRotateText.Text = tb.Value.ToString();
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveJoint, null,
                new MoveJointParameters(_jointNames[(int)lynx.JointNumbers.WristRotate], angle)));
        }

        private void _wristTrackBar_Scroll(object sender, EventArgs e)
        {
            TrackBar tb = (TrackBar)sender;
            float angle = (float)tb.Value;
            _wristText.Text = tb.Value.ToString();
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveJoint, null,
                new MoveJointParameters(_jointNames[(int)lynx.JointNumbers.Wrist], angle)));
        }

        private void _elbowTrackBar_Scroll(object sender, EventArgs e)
        {
            TrackBar tb = (TrackBar)sender;
            float angle = (float)tb.Value;
            _elbowText.Text = tb.Value.ToString();
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveJoint, null,
                new MoveJointParameters(_jointNames[(int)lynx.JointNumbers.Elbow], angle)));
        }

        private void _shoulderTrackBar_Scroll(object sender, EventArgs e)
        {
            TrackBar tb = (TrackBar)sender;
            float angle = (float)tb.Value;
            _shoulderText.Text = tb.Value.ToString();
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveJoint, null,
                new MoveJointParameters(_jointNames[(int)lynx.JointNumbers.Shoulder], angle)));
        }

        private void _baseTrackBar_Scroll(object sender, EventArgs e)
        {
            TrackBar tb = (TrackBar)sender;
            float angle = (float)tb.Value;
            _baseText.Text = tb.Value.ToString();
            _fromWinformPort.Post(new FromWinformMsg(FromWinformMsg.MsgEnum.MoveJoint, null,
                new MoveJointParameters(_jointNames[(int)lynx.JointNumbers.Base], angle)));
        }

        #endregion

    }
}